#include "astarpathfinder.h"
#include <algorithm>
#include <queue>

AStarPathfinder::AStarPathfinder(const std::vector<std::vector<int>>& map)
    : map(map) {}

std::vector<QPoint> AStarPathfinder::findPath(const QPoint& start, const QPoint& end) {
    std::vector<Node*> openSet;
    std::vector<Node*> closedSet;

    openSet.push_back(new Node(start, 0, heuristic(start, end)));

    while (!openSet.empty()) {
        auto current = *std::min_element(openSet.begin(), openSet.end(), [](Node* a, Node* b) {
            return a->fCost < b->fCost;
        });

        if (current->position == end) {
            std::vector<QPoint> path;
            while (current != nullptr) {
                path.push_back(current->position);
                current = current->parent;
            }
            std::reverse(path.begin(), path.end());

            // 清理内存
            for (auto node : openSet) delete node;
            for (auto node : closedSet) delete node;

            return path;
        }

        openSet.erase(std::remove(openSet.begin(), openSet.end(), current), openSet.end());
        closedSet.push_back(current);

        auto neighbors = getNeighbors(current);
        for (auto neighbor : neighbors) {
            if (std::find_if(closedSet.begin(), closedSet.end(), [&](Node* n) {
                    return n->position == neighbor->position;
                }) != closedSet.end()) {
                delete neighbor;
                continue;
            }

            int tentativeGCost = current->gCost + 1;
            auto existing = std::find_if(openSet.begin(), openSet.end(), [&](Node* n) {
                return n->position == neighbor->position;
            });

            if (existing == openSet.end()) {
                neighbor->gCost = tentativeGCost;
                neighbor->hCost = heuristic(neighbor->position, end);
                neighbor->fCost = neighbor->gCost + neighbor->hCost;
                neighbor->parent = current;
                openSet.push_back(neighbor);
            } else {
                if (tentativeGCost < (*existing)->gCost) {
                    (*existing)->gCost = tentativeGCost;
                    (*existing)->fCost = (*existing)->gCost + (*existing)->hCost;
                    (*existing)->parent = current;
                }
                delete neighbor;
            }
        }
    }

    // 清理内存
    for (auto node : openSet) delete node;
    for (auto node : closedSet) delete node;

    return {};
}

int AStarPathfinder::heuristic(const QPoint& a, const QPoint& b) {
    return std::abs(a.x() - b.x()) + std::abs(a.y() - b.y());
}

bool AStarPathfinder::isValid(const QPoint& point) {
    return point.x() >= 0 && point.x() < static_cast<int>(map[0].size()) &&
           point.y() >= 0 && point.y() < static_cast<int>(map.size()) &&
           map[point.y()][point.x()] == 0;
}

std::vector<AStarPathfinder::Node*> AStarPathfinder::getNeighbors(const Node* node) {
    std::vector<Node*> neighbors;
    QPoint directions[] = {QPoint(0, -1), QPoint(0, 1), QPoint(-1, 0), QPoint(1, 0)};

    for (const auto& dir : directions) {
        QPoint newPos = node->position + dir;
        if (isValid(newPos)) {
            neighbors.push_back(new Node(newPos, 0, 0, const_cast<Node*>(node)));
        }
    }

    return neighbors;
}
